#include "main.h"
#include "usart.h"
#include "UPRE_GYRO.h"
#include "UPRE_ROBOT_TASK.h"
struct Gyro gyro = {


        .State=0,
        .Angle_pitch=0,					//俯仰角
        .Angle_roll=0,					//横滚角
        .Angle_azimuth=0,				//方位角
        .W_x=0,		       			  //惯导角速率
        .W_y=0,
        .A_y=0,
        .W_z=0,
        .A_x=0,									//惯导加速度
        .A_z=0,
};
#ifdef GYRO_GI410
/*****************************************************/
//GI410陀螺仪


/*****************************************************/
short Last_Azimuth = 0;						//偏航角上次值
short Delta_Azimuth = 0;					//偏航角变化值
uint8_t Flag_Azimuth = 0;					//偏航角上次值接收标志位
//int fputc(int ch, FILE *f)
//{
//uint8_t temp[1] = {ch};
//HAL_UART_Transmit(&huart5, temp, 1, 0xffff);
//return ch;
//}




static void Gyro_Start(void)
{
    uint8_t cmd_package[9] = {0x5a,0xa5,0x47,0x01,0x01,0x01,0x00,0x4a,0x55};//启动指令
//	temp[0]=0x5a;//帧头
//	temp[1]=0xa5;//帧头
//	temp[2]=0x47;//启动指令
//	temp[3]=1;//启动 0：停止，1：启动
//	temp[4]=1;//判断经纬度
//	temp[5]=1;//判断方位角是否有效
//	temp[6]=0;//预留
//	temp[7]=0x4a;//校验和
//	temp[8]=0x55;//帧尾
    HAL_UART_Transmit(&huart2,cmd_package,9,0xffff);
}
void Gyro_Reset(void)
{
    uint8_t cmd_package[9] = {0x5a,0xa5,0x48,0x00,0x00,0x00,0x00,0x48,0x55};//启动指令
//	bytes[0]=0x5a;//帧头
//	bytes[1]=0xa5;//帧头
//	bytes[2]=0x48;//输入方位角
//	bytes[3]=0x00;//方位角低字节
//	bytes[4]=0x00;//方位角次低字节
//	bytes[5]=0x00;//方位角次高字节
//	bytes[6]=0x00;//方位角高字节
//	bytes[7]=0x48;//校验和
//	bytes[8]=0x55;//帧尾
    HAL_UART_Transmit(&huart2,cmd_package,9,0xffff);
}

void Gyro_Init(void)
{
    Gyro_Start();
}

void cal_GI410()
{

    static uint8_t flag = 1;
    static short Angle_pitch,Angle_roll,Angle_azimuth,W_x,W_y,W_z,A_x,A_y,A_z;

    if(gyro.Gyro_Data[0]==0x5A&&gyro.Gyro_Data[1]==0xA5&&gyro.Gyro_Data[47]==0x55)
    {

        gyro.State   = gyro.Gyro_Data[2];
        Angle_pitch   = (short )( (short)gyro.Gyro_Data[4]<<8|gyro.Gyro_Data[3])/10.f;
        Angle_roll    = (short )( (short)gyro.Gyro_Data[6]<<8|gyro.Gyro_Data[5])/10.f;
        Angle_azimuth = (short )( (short)gyro.Gyro_Data[8]<<8|gyro.Gyro_Data[7])/10.f;


        gyro.Angle_pitch    = ((float)Angle_pitch  ) / 10.f;
        gyro.Angle_roll     = ((float)Angle_roll   ) / 10.f;
        gyro.Angle_azimuth  = ((float)Angle_azimuth) / 10.f;

        W_x      = (short )((short)gyro.Gyro_Data[10]<<8|gyro.Gyro_Data[9])/10.f;
        W_y      = (short )((short)gyro.Gyro_Data[12]<<8|gyro.Gyro_Data[11])/10.f;
        W_z      = (short )((short)gyro.Gyro_Data[14]<<8|gyro.Gyro_Data[13])/10.f;

        gyro.W_x = ((float)W_x) / 10.f;
        gyro.W_y = ((float)W_y) / 10.f;
        gyro.W_z = ((float)W_z) / 10.f;

        A_x      = (short )((short)gyro.Gyro_Data[16]<<8|gyro.Gyro_Data[15])/10.f;
        A_y      = (short )((short)gyro.Gyro_Data[18]<<8|gyro.Gyro_Data[17])/10.f;
        A_z      = (short )((short)gyro.Gyro_Data[20]<<8|gyro.Gyro_Data[19])/10.f;
        gyro.A_x = ((float)A_x) / 10.f;
        gyro.A_y = ((float)A_y) / 10.f;
        gyro.A_z = ((float)A_z) / 10.f;
        if(Flag_Azimuth == 4)
        {
            Last_Azimuth = gyro.Angle_azimuth;
        }
        Delta_Azimuth = gyro.Angle_azimuth - Last_Azimuth;
//			printf("变化:%d\r\n",Delta_Azimuth);
        if(gyro.State == 0x02 && flag == 1)
        {
            flag = 0;
            Gyro_Reset();
            HAL_GPIO_WritePin(GPIOA,GPIO_PIN_8,GPIO_PIN_RESET);
        }
#if Printf_Gyro == 1
			printf("pitch:%f roll:%f yaw:%f \r\n",(double)gyro.Angle_pitch/100.f,(double)gyro.Angle_roll/100.f,(double)gyro.Angle_azimuth/100.f);
#endif

    }


}
#endif

#ifdef GYRO_MINS500
void cal_BW_MINS500() {
static float Last_Yaw = 0;
    if (gyro.Gyro_Data[0] == 0x77 && gyro_sum == gyro.Gyro_Data[13]) {
        gyro.Angle_pitch = (float) ((gyro.Gyro_Data[4] & 0x0f) * 100 + (gyro.Gyro_Data[5] >> 4) * 10 +
                                    (gyro.Gyro_Data[5] & 0x0f) + (gyro.Gyro_Data[6] >> 4) * 0.1f +
                                    (gyro.Gyro_Data[6] & 0x0f) * 0.01f);
        gyro.Angle_roll = (float) ((gyro.Gyro_Data[7] & 0x0f) * 100 + (gyro.Gyro_Data[8] >> 4) * 10 +
                                   (gyro.Gyro_Data[8] & 0x0f) + (gyro.Gyro_Data[9] >> 4) * 0.1f +
                                   (gyro.Gyro_Data[9] & 0x0f) * 0.01f);
        gyro.Angle_azimuth = (float) ((gyro.Gyro_Data[10] & 0x0f) * 100 + (gyro.Gyro_Data[11] >> 4) * 10 +
                                      (gyro.Gyro_Data[11] & 0x0f) + (gyro.Gyro_Data[12] >> 4) * 0.1f +
                                      (gyro.Gyro_Data[12] & 0x0f) * 0.01f);
        if (gyro.Gyro_Data[4] >> 4 == 0x01) {
            gyro.Angle_pitch = -gyro.Angle_pitch;
        }
        if (gyro.Gyro_Data[7] >> 4 == 0x01)
            gyro.Angle_roll = -gyro.Angle_roll;
        if (gyro.Gyro_Data[10] >> 4 == 0x01)
            gyro.Angle_azimuth = -gyro.Angle_azimuth;

        if (gyro.Angle_azimuth <= 360.f && gyro.Angle_azimuth >= 180.f) {
            gyro.Angle_azimuth = gyro.Angle_azimuth - 360.f;
            if(gyro.Angle_azimuth>180.f)
                gyro.Angle_azimuth=180.f;
            if(gyro.Angle_azimuth<-180.f)
                gyro.Angle_azimuth=-180.f;
        }
        if(fabsf(gyro.Angle_azimuth-Last_Yaw)>50.f)
            gyro.Angle_azimuth = Last_Yaw;
        Last_Yaw = gyro.Angle_azimuth;
#if Printf_Gyro == 1
        printf("pitch:%f\troll:%f\tyaw:%f\n",gyro.Angle_pitch,gyro.Angle_roll,gyro.Angle_azimuth);
#endif
    }
}
void GYRO_MINS500_Init()
{
    static uint8_t gyro_rv = 0;
    HAL_UART_Receive_IT(&huart2,&gyro_rv,1);
}
#endif

//接收方式一:串口空闲中断+DMA  放在stm32f4xx_it.c里面
//void USART2_IRQHandler(void)
//{
//    /* USER CODE BEGIN USART2_IRQn 0 */
//    uint32_t temp;
//if((__HAL_UART_GET_FLAG(&huart2,UART_FLAG_IDLE) != RESET))
//    {
//        __HAL_UART_CLEAR_IDLEFLAG(&huart2);
//        HAL_UART_DMAStop(&huart2);
//        temp  =  __HAL_DMA_GET_COUNTER(&hdma_usart2_rx);
////        usb_printf("%d\n",recv_end_flag);
//        rx_len =  BUFFER_SIZE - temp;
//        recv_end_flag = 1;
//    }
//    /* USER CODE END USART2_IRQn 0 */
//    HAL_UART_IRQHandler(&huart2);
//    /* USER CODE BEGIN USART2_IRQn 1 */
//    if(recv_end_flag == 1)
//    {
//        rx_len = 0;
//        recv_end_flag=0;
//        HAL_UART_Receive_DMA(&huart2,gyro.Gyro_Data,BUFFER_SIZE);
//
//    }
//    /* USER CODE END USART2_IRQn 1 */
//}
//接收方式二:读取串口寄存器数值并处理数据
//void RX_Gyro_IRQ(void)
//{
//	static uint8_t i = 1;
//	static uint8_t sum = 0;
//	static uint8_t Navigator_Flag = 0;
//	static int LastYaw = 0;
//	static int deltaYaw = 0;
//	static int runtime = 0;
//	static uint8_t Res = 0;
//	Res = huart2.Instance->DR;
//	if(Navigator_Flag == 2)
//	{
//		gyro.Gyro_Data[i++] = huart2.Instance->DR;
//		if(i==49)
//		{
//				Navigator_Flag=0;
//				for(int k = 3;k <= 46;k++)
//			{
//				sum+=gyro.Gyro_Data[k];
//			}
//
//			if(gyro.Gyro_Data[48] == 0x55 && sum == gyro.Gyro_Data[47])
//			{
//				gyro.State =gyro.Gyro_Data[3];												//0-监控状态 1-静态对准 2-INS导航	//状态位
//				gyro.Angle_pitch = 		((short)gyro.Gyro_Data[5]<<8) | (short)gyro.Gyro_Data[4];
//				gyro.Angle_roll = 		((short)gyro.Gyro_Data[7]<<8) | (short)gyro.Gyro_Data[6];
//				gyro.Angle_azimuth =  ((short)gyro.Gyro_Data[9]<<8) | (short)gyro.Gyro_Data[8];
//				gyro.W_x = 						((short)gyro.Gyro_Data[11]<<8) | (short)gyro.Gyro_Data[10];
//				gyro.W_y = 						((short)gyro.Gyro_Data[13]<<8) | (short)gyro.Gyro_Data[12];
//				gyro.W_z = 						((short)gyro.Gyro_Data[15]<<8) | (short)gyro.Gyro_Data[14];
//				gyro.A_x=							(short)(gyro.Gyro_Data[17]<<8)|gyro.Gyro_Data[16];
//				gyro.A_y=							(short)(gyro.Gyro_Data[19]<<8)|gyro.Gyro_Data[18];
//				gyro.A_z=							(short)(gyro.Gyro_Data[21]<<8)|gyro.Gyro_Data[20];
//
//				if(runtime == 0)
//				{
//					LastYaw = gyro.Angle_azimuth;
//					runtime = 1;
//				}
//				deltaYaw = gyro.Angle_azimuth - LastYaw;
//				gyro.Delata_Angle = deltaYaw;
//			}
//			sum = 0;
//		}
//	}
//	//帧头校验
//	else switch(huart2.Instance->DR)
//	{
//		case 0x5A:
//			if(Navigator_Flag == 0)
//		{
//			Navigator_Flag = 1;
//		}
//			break;
//		case 0xA5:
//			if(Navigator_Flag == 1)
//			{
//				Navigator_Flag = 2;
//				i = 3;
//			}
//			break;
//		default:
//			Navigator_Flag = 0;
//			break;
//	}
//	HAL_UART_Receive_IT(&huart2,&Res,1);
//}

//void get_GI410()
//{
//	static short Count = 0;
//	static uint8_t lastrxdata;
//	gyro.Gyro_Data[Count] = rxgi410;
//	if(lastrxdata == 0x55 && rxgi410 == 0x5A || Count>0)
//	{
//		Count++;
//	}
//	if(Count == 47)
//	{
//		Count = 0;
//		if(gyro.Gyro_Data[47-1] == 0x55)
//		{
//			cal_GI410(gyro.Gyro_Data);
//		}
//	}
//	lastrxdata = rxgi410;
////	printf("%c",rxgi410);
//}

